/* -*- c++ -*- */

#ifndef _PUNTO_H
#define _PUNTO_H
#include <math.h>
#include "defs.h"

#define PUNTO_X    0
#define PUNTO_Y    1
#define PUNTO_Z    2
#define PUNTO_XYZ  3

class PuntoDouble;

class Punto {
private:
  float _coord[ PUNTO_XYZ]; // ( x, y, z) coords
public:
  Punto( const float x, const float y = 0.0f, const float z = 0.0f)
    // : _hecho( false), _modulo( 0.0f)
  {
    _coord[ PUNTO_X] = x;
    _coord[ PUNTO_Y] = y;
    _coord[ PUNTO_Z] = z;
  };

  Punto( const float *v)
    // : _hecho( false), _modulo( 0.0f)
  {
    _coord[ PUNTO_X] = v[ PUNTO_X];
    _coord[ PUNTO_Y] = v[ PUNTO_Y];
    _coord[ PUNTO_Z] = v[ PUNTO_Z];
  };

  Punto( const double *v)
    // : _hecho( false), _modulo( 0.0f)
  {
    _coord[ PUNTO_X] = ( float)v[ PUNTO_X];
    _coord[ PUNTO_Y] = ( float)v[ PUNTO_Y];
    _coord[ PUNTO_Z] = ( float)v[ PUNTO_Z];
  };

  Punto( void) //{ }
    //	    : _hecho( false), _modulo( 0.0f)
  { _coord[ PUNTO_X] = _coord[ PUNTO_Y] = _coord[ PUNTO_Z] = 0.0f; }

  ~Punto() { };

  Punto( const Punto &p) //: _hecho( p._hecho), _modulo( p._modulo)
  {
    _coord[ PUNTO_X] = p._coord[ PUNTO_X];
    _coord[ PUNTO_Y] = p._coord[ PUNTO_Y];
    _coord[ PUNTO_Z] = p._coord[ PUNTO_Z];
  };

  // Punto( const PuntoDouble &p);

  float &x( void) { return _coord[ PUNTO_X]; };
  float &y( void) { return _coord[ PUNTO_Y]; };
  float &z( void) { return _coord[ PUNTO_Z]; };

  float x( void) const { return _coord[ PUNTO_X]; };
  float y( void) const { return _coord[ PUNTO_Y]; };
  float z( void) const { return _coord[ PUNTO_Z]; };

  float mod( void) const
  {
    return ( float)sqrt( _coord[ PUNTO_X] * _coord[ PUNTO_X] +
                         _coord[ PUNTO_Y] * _coord[ PUNTO_Y] +
                         _coord[ PUNTO_Z] * _coord[ PUNTO_Z]);
  }
  float modf( void) const
  {
    return sqrtf( _coord[ PUNTO_X] * _coord[ PUNTO_X] +
                  _coord[ PUNTO_Y] * _coord[ PUNTO_Y] +
                  _coord[ PUNTO_Z] * _coord[ PUNTO_Z]);
  }

  float mod2( void) const
  {
    return ( float)( _coord[ PUNTO_X] * _coord[ PUNTO_X] +
		    _coord[ PUNTO_Y] * _coord[ PUNTO_Y] +
		    _coord[ PUNTO_Z] * _coord[ PUNTO_Z]);
  }

  float length( void) const
  {
    return ( float)( fabs( _coord[ PUNTO_X]) +
		    fabs( _coord[ PUNTO_Y]) +
		    fabs( _coord[ PUNTO_Z]));
  }

  Punto &abs( void) {
    _coord[ PUNTO_X] = ( float)fabs( _coord[ PUNTO_X]);
    _coord[ PUNTO_Y] = ( float)fabs( _coord[ PUNTO_Y]);
    _coord[ PUNTO_Z] = ( float)fabs( _coord[ PUNTO_Z]);
    return *this;
  }

  Punto &normalize( void)
  {
    float l = length();
    if ( ( l <= 0.0f) || ( l == 1.0))
      return *this;
    l = mod();
    float il = 1.0f / l;
    x() *= il;
    y() *= il;
    z() *= il;
    return *this;
  }

  // Punto &minimo( const PuntoDouble &p);
  // Punto &maximo( const PuntoDouble &p);

  Punto &minimo( const Punto &p) {
    if ( p.x() < x()) x() = p.x();
    if ( p.y() < y()) y() = p.y();
    if ( p.z() < z()) z() = p.z();
    return *this;
  }

  Punto &maximo( const Punto &p) {
    if ( p.x() > x()) x() = p.x();
    if ( p.y() > y()) y() = p.y();
    if ( p.z() > z()) z() = p.z();
    return *this;
  }

//   void pinta( void) const {
// #ifdef DOBLE
//     glVertex3dv( _coord);
// #else
//     glVertex3fv( _coord);
// #endif
//   }
// 
//   void pintar( void) const {
// #ifdef DOBLE
//     glVertex3dv( _coord);
// #else
//     glVertex3fv( _coord);
// #endif
//   }

  Punto &set( float x, float y = 0.0f, float z = 0.0f)
  {
    _coord[ PUNTO_X] = x;
    _coord[ PUNTO_Y] = y;
    _coord[ PUNTO_Z] = z;
    return *this;
  };

  Punto &set( double x, double y = 0.0f, double z = 0.0f)
  {
    _coord[ PUNTO_X] = ( float)x;
    _coord[ PUNTO_Y] = ( float)y;
    _coord[ PUNTO_Z] = ( float)z;
    return *this;
  };

  Punto &set( const float *vector) {
    _coord[ PUNTO_X] = vector[ 0];
    _coord[ PUNTO_Y] = vector[ 1];
    _coord[ PUNTO_Z] = vector[ 2];
    return *this;
  };

  Punto &set( const double *vector) {
    _coord[ PUNTO_X] = ( float)vector[ 0];
    _coord[ PUNTO_Y] = ( float)vector[ 1];
    _coord[ PUNTO_Z] = ( float)vector[ 2];
    return *this;
  };

  Punto &reset( void) { set( 0.0f, 0.0f, 0.0f); return *this;}

  Punto &setx( float x = 0.0f)
  { this->x() = x; return *this; };
  Punto &sety( float y = 0.0f)
  { this->y() = y; return *this; };
  Punto &setz( float z = 0.0f)
  { this->z() = z; return *this; };

  operator float *() const { return ( float *)_coord; }
  operator float *() { return ( float *)_coord; }

  Punto operator-() const
  {
    Punto p( - x(), - y(), - z());
    return p;
  }

  bool operator!() { return ( !x() && !y() && !z()); }

  Punto &operator=( const Punto &p)
  {
    x() = p.x();
    y() = p.y();
    z() = p.z();
    return *this;
  }

  // Punto &operator=( const PuntoDouble &p);

  Punto &operator=( const float &a)
  {
    x() = a;
    y() = a;
    z() = a;
    return *this;
  }

  Punto &operator=( const float *v)
  {
    x() = v[ 0];
    y() = v[ 1];
    z() = v[ 2];
    return *this;
  }

  Punto &operator+=( const Punto &p)
  {
    x() += p.x();
    y() += p.y();
    z() += p.z();
    return *this;
  }

  Punto &operator-=( const Punto &p)
  {
    x() -= p.x();
    y() -= p.y();
    z() -= p.z();
    return *this;
  }

  int dentro( const Punto &min, const Punto &max) const {
    if ( ( min.x() <= x()) && ( x() <= max.x()) &&
	 ( min.y() <= y()) && ( y() <= max.y()) &&
	 ( min.z() <= z()) && ( z() <= max.z()))
      return 1;
    else
      return 0;
  }

  Punto spherical_coordinates() const;
  
  /*
   * !!! Viva el AMIGUISMO !!!
   */

  inline friend Punto operator+( const Punto &p, const Punto &q);
  inline friend Punto operator-( const Punto &p, const Punto &q);

  // Producto escalar
  // inline friend float operator*( const Punto &p, const Punto &q);
  inline friend float scalar_product( const Punto &p, const Punto &q);
  inline friend Punto component_product( const Punto &p, const Punto &q);

  inline friend Punto operator*( const float &a, const Punto &q);
  inline friend Punto operator*( const Punto &q, const float &a);
  inline friend Punto operator*( const double &a, const Punto &q);
  inline friend Punto operator*( const Punto &q, const double &a);
  inline friend float operator/( const float &a, Punto &q);
  inline friend double operator/( const double &a, Punto &q);

  inline friend Punto operator^( const Punto &p, const Punto &q);

  inline friend bool operator==( const Punto &p, const Punto &q);
  inline friend bool operator!=( const Punto &p, const Punto &q);
};

inline Punto Punto::spherical_coordinates() const {
  float r = this->mod();
  float theta = acosf( _coord[ PUNTO_Z] / r);
  float phi = atan2f( _coord[ PUNTO_Y], _coord[ PUNTO_X]);
  return Punto( r, theta, phi);
}


inline Punto operator+( const Punto &p, const Punto &q) {
  Punto r( p);
  r += q;
  return r;
}

inline Punto operator-( const Punto &p, const Punto &q) {
  Punto r( p);
  r -= q;
  return r;
}

// Producto vectorial
inline Punto operator^( const Punto &p, const Punto &q) {
  return Punto( p.y() * q.z() - p.z() * q.y(),  p.z() * q.x() - p.x() * q.z(),  p.x() * q.y() - p.y() * q.x());
}

// Producto escalar
// inline float operator*( const Punto &p, const Punto &q) {
//   return ( float)( p.x() * q.x() + p.y() * q.y() + p.z() * q.z());
// }

inline float scalar_product( const Punto &p, const Punto &q) {
  return ( float)( p.x() * q.x() + p.y() * q.y() + p.z() * q.z());
}

inline Punto component_product( const Punto &p, const Punto &q) {
  return Punto( p.x() * q.x(), p.y() * q.y(), p.z() * q.z());
}

inline Punto operator*( const float &a, const Punto &q) {
  Punto r( a * q.x(), a * q.y(), a * q.z());
  return r;
}

inline Punto operator*( const Punto &q, const float &a) {
  Punto r( a * q.x(), a * q.y(), a * q.z());
  return r;
}

inline Punto operator*( const double &a, const Punto &q) {
  Punto r( ( float)a * q.x(), ( float)a * q.y(), ( float)a * q.z());
  return r;
}

inline Punto operator*( const Punto &q, const double &a) {
  Punto r( ( float)a * q.x(), ( float)a * q.y(), ( float)a * q.z());
  return r;
}

inline Punto operator*=( const float &a, Punto &q) {
  return q.set( a * q.x(), a * q.y(), a * q.z());
}

inline Punto operator*=( Punto &q, const float &a) {
  return q.set( a * q.x(), a * q.y(), a * q.z());
}

inline Punto operator*=( const double &a, Punto &q) {
  return q.set( ( float)a * q.x(), ( float)a * q.y(), ( float)a * q.z());
}

inline Punto operator*=( Punto &q, const double &a) {
  return q.set( ( float)a * q.x(), ( float)a * q.y(), ( float)a * q.z());
}


inline Punto operator/( const Punto &q, const float &a) {
  float d( a);

  if ( !a)
    d = 1;
  Punto r( q.x() / d, q.y() / d, q.z() / d);
  return r;
}

inline Punto operator/( const Punto &q, const double &a) {
  double d( a);

  if ( !a)
    d = 1;
  Punto r( q.x() / ( float)d, q.y() / ( float)d, q.z() / ( float)d);
  return r;
}

inline bool operator==( const Punto &p, const Punto &q) {
  Punto dif = p - q;
  float max = ( float)fabs( p.x());
  if ( fabs( p.y()) > max) max = ( float)fabs( p.y());
  if ( fabs( p.z()) > max) max = ( float)fabs( p.z());
  if ( fabs( q.x()) > max) max = ( float)fabs( q.x());
  if ( fabs( q.y()) > max) max = ( float)fabs( q.y());
  if ( fabs( q.z()) > max) max = ( float)fabs( q.z());
  float eps = ( float)( max * EPSILON_CMP);
  if ( dif.mod2() > eps * eps + CASI_CERO_2) return false;
  return true;
}

inline bool operator!=( const Punto &p, const Punto &q) {
  return ( !( p == q));
}

inline Punto calc_normal( const Punto &p1, const Punto &p2, const Punto &p3) {
  Punto normal;
  Punto u,v;
  u = p2 - p1;
  v = p3 - p1;
  float x = u.y() * v.z() - u.z() * v.y();
  float y = u.z() * v.x() - u.x() * v.z();
  float z = u.x() * v.y() - u.y() * v.x();
  normal.set( x, y, z);
  normal.normalize();
  return normal;
}

////////////////////////////////////////////////////////////////////////
/// PuntoDouble
////////////////////////////////////////////////////////////////////////

class PuntoDouble {
private:
  double _coord[ PUNTO_XYZ]; // ( x, y, z) coords
public:
  PuntoDouble( double x, double y = 0.0f, double z = 0.0f)
    // : _hecho( false), _modulo( 0.0f)
  {
    _coord[ PUNTO_X] = x;
    _coord[ PUNTO_Y] = y;
    _coord[ PUNTO_Z] = z;
  };

  // PuntoDouble( float *v)
  //   // : _hecho( false), _modulo( 0.0f)
  // {
  //   _coord[ PUNTO_X] = v[ PUNTO_X];
  //   _coord[ PUNTO_Y] = v[ PUNTO_Y];
  //   _coord[ PUNTO_Z] = v[ PUNTO_Z];
  // };

  PuntoDouble( double *v)
    // : _hecho( false), _modulo( 0.0f)
  {
    _coord[ PUNTO_X] = v[ PUNTO_X];
    _coord[ PUNTO_Y] = v[ PUNTO_Y];
    _coord[ PUNTO_Z] = v[ PUNTO_Z];
  };

  PuntoDouble( void) //{ }
    //	    : _hecho( false), _modulo( 0.0f)
  { _coord[ PUNTO_X] = _coord[ PUNTO_Y] = _coord[ PUNTO_Z] = 0.0f; }

  ~PuntoDouble() { };

  PuntoDouble( const PuntoDouble &p) //: _hecho( p._hecho), _modulo( p._modulo)
  {
    _coord[ PUNTO_X] = p._coord[ PUNTO_X];
    _coord[ PUNTO_Y] = p._coord[ PUNTO_Y];
    _coord[ PUNTO_Z] = p._coord[ PUNTO_Z];
  };

  PuntoDouble( const Punto &p);

  double &x( void) { return _coord[ PUNTO_X]; };
  double &y( void) { return _coord[ PUNTO_Y]; };
  double &z( void) { return _coord[ PUNTO_Z]; };

  double x( void) const { return _coord[ PUNTO_X]; };
  double y( void) const { return _coord[ PUNTO_Y]; };
  double z( void) const { return _coord[ PUNTO_Z]; };

  double mod( void) const
  {
    return ( double)sqrt( _coord[ PUNTO_X] * _coord[ PUNTO_X] +
			_coord[ PUNTO_Y] * _coord[ PUNTO_Y] +
			_coord[ PUNTO_Z] * _coord[ PUNTO_Z]);
  }

  double mod2( void) const
  {
    return ( double)( _coord[ PUNTO_X] * _coord[ PUNTO_X] +
		    _coord[ PUNTO_Y] * _coord[ PUNTO_Y] +
		    _coord[ PUNTO_Z] * _coord[ PUNTO_Z]);
  }

  double length( void) const
  {
    return ( double)( fabs( _coord[ PUNTO_X]) +
		    fabs( _coord[ PUNTO_Y]) +
		    fabs( _coord[ PUNTO_Z]));
  }

  PuntoDouble &abs( void) {
    _coord[ PUNTO_X] = fabs( _coord[ PUNTO_X]);
    _coord[ PUNTO_Y] = fabs( _coord[ PUNTO_Y]);
    _coord[ PUNTO_Z] = fabs( _coord[ PUNTO_Z]);
    return *this;
  }

  PuntoDouble &normalize( void)
  {
    double l = length();
    if ( ( l <= 0.0f) || ( l == 1.0))
      return *this;
    l = mod();
    double il = 1.0 / l;
    x() *= il;
    y() *= il;
    z() *= il;
    return *this;
  }

  PuntoDouble &minimo( const Punto &p);
  PuntoDouble &maximo( const Punto &p);

  PuntoDouble &minimo( const PuntoDouble &p) {
    if ( p.x() < x()) x() = p.x();
    if ( p.y() < y()) y() = p.y();
    if ( p.z() < z()) z() = p.z();
    return *this;
  }

  PuntoDouble &maximo( const PuntoDouble &p) {
    if ( p.x() > x()) x() = p.x();
    if ( p.y() > y()) y() = p.y();
    if ( p.z() > z()) z() = p.z();
    return *this;
  }

  // void pinta( void) const {
  //   glVertex3dv( _coord);
  // }
  // 
  // void pintar( void) const {
  //   glVertex3dv( _coord);
  // }

  PuntoDouble &set( double x, double y = 0.0f, double z = 0.0f)
  {
    _coord[ PUNTO_X] = x;
    _coord[ PUNTO_Y] = y;
    _coord[ PUNTO_Z] = z;
    return *this;
  };

  PuntoDouble &set( const float *vector) {
    _coord[ PUNTO_X] = vector[ 0];
    _coord[ PUNTO_Y] = vector[ 1];
    _coord[ PUNTO_Z] = vector[ 2];
    return *this;
  };

  PuntoDouble &set( const double *vector) {
    _coord[ PUNTO_X] = vector[ 0];
    _coord[ PUNTO_Y] = vector[ 1];
    _coord[ PUNTO_Z] = vector[ 2];
    return *this;
  };

  PuntoDouble &reset( void) { set( 0.0f, 0.0f, 0.0f); return *this;}

  PuntoDouble &setx( double x = 0.0f)
  { this->x() = x; return *this; };
  PuntoDouble &sety( double y = 0.0f)
  { this->y() = y; return *this; };
  PuntoDouble &setz( double z = 0.0f)
  { this->z() = z; return *this; };

  operator double *() const { return ( double *)_coord; }
  operator double *() { return ( double *)_coord; }

  PuntoDouble operator-() const
  {
    PuntoDouble p( - x(), - y(), - z());
    return p;
  }

  bool operator!() { return ( !x() && !y() && !z()); }

  PuntoDouble &operator=( const PuntoDouble &p)
  {
    x() = p.x();
    y() = p.y();
    z() = p.z();
    return *this;
  }

  PuntoDouble &operator=( const Punto &p);

  PuntoDouble &operator=( const double &a)
  {
    x() = a;
    y() = a;
    z() = a;
    return *this;
  }

  PuntoDouble &operator=( const float *v)
  {
    x() = v[ 0];
    y() = v[ 1];
    z() = v[ 2];
    return *this;
  }

  PuntoDouble &operator+=( const PuntoDouble &p)
  {
    x() += p.x();
    y() += p.y();
    z() += p.z();
    return *this;
  }

  PuntoDouble &operator-=( const PuntoDouble &p)
  {
    x() -= p.x();
    y() -= p.y();
    z() -= p.z();
    return *this;
  }

  int dentro( const PuntoDouble &min, const PuntoDouble &max) const {
    if ( ( min.x() <= x()) && ( x() <= max.x()) &&
	 ( min.y() <= y()) && ( y() <= max.y()) &&
	 ( min.z() <= z()) && ( z() <= max.z()))
      return 1;
    else
      return 0;
  }
	
  /*
   * !!! Viva el AMIGUISMO !!!
   */

  inline friend PuntoDouble operator+( const PuntoDouble &p, const PuntoDouble &q);
  inline friend PuntoDouble operator-( const PuntoDouble &p, const PuntoDouble &q);
  // inline friend PuntoDouble operator+( const PuntoDouble &p, const Punto &q);
  // inline friend PuntoDouble operator-( const PuntoDouble &p, const Punto &q);
  // inline friend PuntoDouble operator+( const Punto &p, const PuntoDouble &q);
  // inline friend PuntoDouble operator-( const Punto &p, const PuntoDouble &q);

  // Producto escalar
  // inline friend double operator*( const PuntoDouble &p, const PuntoDouble &q);
  inline friend double scalar_product( const PuntoDouble &p, const PuntoDouble &q);
  inline friend PuntoDouble component_product( const PuntoDouble &p, const PuntoDouble &q);
  // inline friend double operator*( const Punto &p, const PuntoDouble &q);
  // inline friend double operator*( const PuntoDouble &p, const Punto &q);

  inline friend PuntoDouble operator*( const float &a, const PuntoDouble &q);
  inline friend PuntoDouble operator*( const PuntoDouble &q, const float &a);
  inline friend PuntoDouble operator*( const double &a, const PuntoDouble &q);
  inline friend PuntoDouble operator*( const PuntoDouble &q, const double &a);
  inline friend float operator/( const float &a, PuntoDouble &q);
  inline friend double operator/( const double &a, PuntoDouble &q);

  inline friend PuntoDouble operator^( const PuntoDouble &p, const PuntoDouble &q);

  inline friend bool operator==( const PuntoDouble &p, const PuntoDouble &q);
  inline friend bool operator!=( const PuntoDouble &p, const PuntoDouble &q);
};

inline PuntoDouble operator+( const PuntoDouble &p, const PuntoDouble &q) {
  PuntoDouble r( p);
  r += q;
  return r;
}

inline PuntoDouble operator-( const PuntoDouble &p, const PuntoDouble &q) {
  PuntoDouble r( p);
  r -= q;
  return r;
}

// Producto vectorial
inline PuntoDouble operator^( const PuntoDouble &p, const PuntoDouble &q) {
  return PuntoDouble( p.y() * q.z() - p.z() * q.y(),  p.z() * q.x() - p.x() * q.z(),  p.x() * q.y() - p.y() * q.x());
}

// // Producto escalar
// inline double operator*( const PuntoDouble &p, const PuntoDouble &q) {
//   return ( double)( p.x() * q.x() + p.y() * q.y() + p.z() * q.z());
// }

inline double scalar_product( const PuntoDouble &p, const PuntoDouble &q) {
  return ( double)( p.x() * q.x() + p.y() * q.y() + p.z() * q.z());
}

inline PuntoDouble component_product( const PuntoDouble &p, const PuntoDouble &q) {
  return PuntoDouble( p.x() * q.x(), p.y() * q.y(), p.z() * q.z());
}

inline PuntoDouble operator*( const float &a, const PuntoDouble &q) {
  PuntoDouble r( a * q.x(), a * q.y(), a * q.z());
  return r;
}

inline PuntoDouble operator*( const PuntoDouble &q, const float &a) {
  PuntoDouble r( a * q.x(), a * q.y(), a * q.z());
  return r;
}

inline PuntoDouble operator*( const double &a, const PuntoDouble &q) {
  PuntoDouble r( a * q.x(), a * q.y(), a * q.z());
  return r;
}

inline PuntoDouble operator*( const PuntoDouble &q, const double &a) {
  PuntoDouble r( a * q.x(), a * q.y(), a * q.z());
  return r;
}

inline PuntoDouble operator*=( const float &a, PuntoDouble &q) {
  return q.set( a * q.x(), a * q.y(), a * q.z());
}

inline PuntoDouble operator*=( PuntoDouble &q, const float &a) {
  return q.set( a * q.x(), a * q.y(), a * q.z());
}

inline PuntoDouble operator*=( const double &a, PuntoDouble &q) {
  return q.set( a * q.x(), a * q.y(), a * q.z());
}

inline PuntoDouble operator*=( PuntoDouble &q, const double &a) {
  return q.set( a * q.x(), a * q.y(), a * q.z());
}


inline PuntoDouble operator/( const PuntoDouble &q, const float &a) {
  float d( a);

  if ( !a)
    d = 1;
  PuntoDouble r( q.x() / d, q.y() / d, q.z() / d);
  return r;
}

inline PuntoDouble operator/( const PuntoDouble &q, const double &a) {
  double d( a);

  if ( !a)
    d = 1;
  PuntoDouble r( q.x() / d, q.y() / d, q.z() / d);
  return r;
}

inline bool operator==( const PuntoDouble &p, const PuntoDouble &q) {
  PuntoDouble dif = p - q;
  double max = fabs( p.x());
  if ( fabs( p.y()) > max) max = fabs( p.y());
  if ( fabs( p.z()) > max) max = fabs( p.z());
  if ( fabs( q.x()) > max) max = fabs( q.x());
  if ( fabs( q.y()) > max) max = fabs( q.y());
  if ( fabs( q.z()) > max) max = fabs( q.z());
  double eps = max * EPSILON_CMP;
  if ( dif.mod2() > eps * eps + CASI_CERO_2) return false;
  return true;
}

inline bool operator!=( const PuntoDouble &p, const PuntoDouble &q) {
  return ( !( p == q));
}

typedef Punto Vector;

// class float4 {
// public:
//   float4() {};
//   float4( const Punto &p_in, float d_in = 0.0f) {
//     a = p_in.x();
//     b = p_in.y();
//     c = p_in.z();
//     d = d_in;
//   }
//   float4( float aa, float bb, float cc, float dd) {
//     a = aa;
//     b = bb;
//     c = cc;
//     d = dd;
//   }
//   float4 &operator=( const Punto &p) {
//     a = p.x();
//     b = p.y();
//     c = p.z();
//     return *this;
//   }
//   float4 &operator=( const float4 &p) {
//     a = p.a;
//     b = p.b;
//     c = p.c;
//     d = p.d;
//     return *this;
//   }
// 
// public:
//   float a, b, c, d;
// };


// Matrices en OGL: no se guarda como en C/C++ si no como en fortran
class GLMatrix {
private:
  float _d[ 16];

public:
  GLMatrix( void) {}
  GLMatrix( const float ini) {
    _d[  0] = ini; _d[  1] = ini; _d[  2] = ini; _d[  3] = ini;
    _d[  4] = ini; _d[  5] = ini; _d[  6] = ini; _d[  7] = ini;
    _d[  8] = ini; _d[  9] = ini; _d[ 10] = ini; _d[ 11] = ini;
    _d[ 12] = ini; _d[ 13] = ini; _d[ 14] = ini; _d[ 15] = ini;
  };
  GLMatrix( const GLMatrix &m) {
    _d[  0] = m._d[  0]; _d[  1] = m._d[  1]; _d[  2] = m._d[  2]; _d[  3] = m._d[  3];
    _d[  4] = m._d[  4]; _d[  5] = m._d[  5]; _d[  6] = m._d[  6]; _d[  7] = m._d[  7];
    _d[  8] = m._d[  8]; _d[  9] = m._d[  9]; _d[ 10] = m._d[ 10]; _d[ 11] = m._d[ 11];
    _d[ 12] = m._d[ 12]; _d[ 13] = m._d[ 13]; _d[ 14] = m._d[ 14]; _d[ 15] = m._d[ 15];
  }
  GLMatrix( float *const m) {
    _d[  0] = m[  0]; _d[  1] = m[  1]; _d[  2] = m[  2]; _d[  3] = m[  3];
    _d[  4] = m[  4]; _d[  5] = m[  5]; _d[  6] = m[  6]; _d[  7] = m[  7];
    _d[  8] = m[  8]; _d[  9] = m[  9]; _d[ 10] = m[ 10]; _d[ 11] = m[ 11];
    _d[ 12] = m[ 12]; _d[ 13] = m[ 13]; _d[ 14] = m[ 14]; _d[ 15] = m[ 15];
  }
  GLMatrix( const float *m) {
    _d[  0] = m[  0]; _d[  1] = m[  1]; _d[  2] = m[  2]; _d[  3] = m[  3];
    _d[  4] = m[  4]; _d[  5] = m[  5]; _d[  6] = m[  6]; _d[  7] = m[  7];
    _d[  8] = m[  8]; _d[  9] = m[  9]; _d[ 10] = m[ 10]; _d[ 11] = m[ 11];
    _d[ 12] = m[ 12]; _d[ 13] = m[ 13]; _d[ 14] = m[ 14]; _d[ 15] = m[ 15];
  }
  GLMatrix( const float i0, const float i1, const float i2, const float i3,
	    const float i4, const float i5, const float i6, const float i7,
	    const float i8, const float i9, const float i10, const float i11,
	    const float i12, const float i13, const float i14, const float i15) {
    _d[  0] = i0; _d[  1] = i1; _d[  2] = i2; _d[  3] = i3;
    _d[  4] = i4; _d[  5] = i5; _d[  6] = i6; _d[  7] = i7;
    _d[  8] = i8; _d[  9] = i9; _d[ 10] = i10; _d[ 11] = i11;
    _d[ 12] = i12; _d[ 13] = i13; _d[ 14] = i14; _d[ 15] = i15;
  };

  ~GLMatrix() {}

  GLMatrix &Id() {
    _d[  0] = 1.0;
    _d[  1] = _d[  2] = _d[  3] = _d[  4] = 0.0f;
    _d[  5] = 1.0;
    _d[  6] = _d[  7] = _d[  8] = _d[  9] = 0.0f;
    _d[ 10] = 1.0;
    _d[ 11] = _d[ 12] = _d[ 13] = _d[ 14] = 0.0f;
    _d[ 15] = 1.0;
    return *this;
  }

  float operator[] ( const int i) const { return _d[ i];}
  float &operator[] ( const int i) { return _d[ i];}

  float operator() ( const int f, const int c) const { return _d[ f + ( c << 2)];}
  float &operator() ( const int f, const int c) { return _d[ f + ( c << 2)];}

  operator float *() { return _d; }

  void limpia( void) {
#define FLOAT_EPS  1e-6
    if ( fabs( _d[  0]) < FLOAT_EPS) _d[  0] = 0.0f;
    if ( fabs( _d[  1]) < FLOAT_EPS) _d[  1] = 0.0f;
    if ( fabs( _d[  2]) < FLOAT_EPS) _d[  2] = 0.0f;
    if ( fabs( _d[  3]) < FLOAT_EPS) _d[  3] = 0.0f;
    if ( fabs( _d[  4]) < FLOAT_EPS) _d[  4] = 0.0f;
    if ( fabs( _d[  5]) < FLOAT_EPS) _d[  5] = 0.0f;
    if ( fabs( _d[  6]) < FLOAT_EPS) _d[  6] = 0.0f;
    if ( fabs( _d[  7]) < FLOAT_EPS) _d[  7] = 0.0f;
    if ( fabs( _d[  8]) < FLOAT_EPS) _d[  8] = 0.0f;
    if ( fabs( _d[  9]) < FLOAT_EPS) _d[  9] = 0.0f;
    if ( fabs( _d[ 10]) < FLOAT_EPS) _d[ 10] = 0.0f;
    if ( fabs( _d[ 11]) < FLOAT_EPS) _d[ 11] = 0.0f;
    if ( fabs( _d[ 12]) < FLOAT_EPS) _d[ 12] = 0.0f;
    if ( fabs( _d[ 13]) < FLOAT_EPS) _d[ 13] = 0.0f;
    if ( fabs( _d[ 14]) < FLOAT_EPS) _d[ 14] = 0.0f;
    if ( fabs( _d[ 15]) < FLOAT_EPS) _d[ 15] = 0.0f;
  }

  GLMatrix &operator= ( const GLMatrix &m) {
    _d[  0] = m._d[  0]; _d[  1] = m._d[  1]; _d[  2] = m._d[  2]; _d[  3] = m._d[  3];
    _d[  4] = m._d[  4]; _d[  5] = m._d[  5]; _d[  6] = m._d[  6]; _d[  7] = m._d[  7];
    _d[  8] = m._d[  8]; _d[  9] = m._d[  9]; _d[ 10] = m._d[ 10]; _d[ 11] = m._d[ 11];
    _d[ 12] = m._d[ 12]; _d[ 13] = m._d[ 13]; _d[ 14] = m._d[ 14]; _d[ 15] = m._d[ 15];
    return *this;
  }

  GLMatrix &operator= ( float *const m) {
    _d[  0] = m[  0]; _d[  1] = m[  1]; _d[  2] = m[  2]; _d[  3] = m[  3];
    _d[  4] = m[  4]; _d[  5] = m[  5]; _d[  6] = m[  6]; _d[  7] = m[  7];
    _d[  8] = m[  8]; _d[  9] = m[  9]; _d[ 10] = m[ 10]; _d[ 11] = m[ 11];
    _d[ 12] = m[ 12]; _d[ 13] = m[ 13]; _d[ 14] = m[ 14]; _d[ 15] = m[ 15];
    return *this;
  }

  GLMatrix operator-() {
    return GLMatrix( -_d[  0], -_d[  1], -_d[  2], -_d[  3], -_d[  4], -_d[  5], -_d[  6], -_d[  7],
		     -_d[  8], -_d[  9], -_d[ 10], -_d[ 11], -_d[ 12], -_d[ 13], -_d[ 14], -_d[ 15]);
  }

  inline bool get_inverse( float *out) const;
  inline bool get_transpose( GLMatrix &out) const;
  inline float get_determinant() const;

  inline void sort();
  
  // friends: como la serie

  inline friend GLMatrix operator*( const GLMatrix &a, const GLMatrix &b);
  inline friend GLMatrix operator*( float *const a, const GLMatrix &b);
  inline friend GLMatrix operator*( const GLMatrix &a, float *const b);
  inline friend Punto operator*( const GLMatrix &a, const Punto &p);

};


// MultMatVec de matrices OGL == float[ 16], c = a * b
inline GLMatrix operator*( const GLMatrix &a, const GLMatrix &b) {
  return GLMatrix( b._d[  0] * a._d[  0] + b._d[  1] * a._d[  4] + b._d[  2] * a._d[  8] + b._d[  3] * a._d[ 12],
		   b._d[  0] * a._d[  1] + b._d[  1] * a._d[  5] + b._d[  2] * a._d[  9] + b._d[  3] * a._d[ 13],
		   b._d[  0] * a._d[  2] + b._d[  1] * a._d[  6] + b._d[  2] * a._d[ 10] + b._d[  3] * a._d[ 14],
		   b._d[  0] * a._d[  3] + b._d[  1] * a._d[  7] + b._d[  2] * a._d[ 11] + b._d[  3] * a._d[ 15],
		
		   b._d[  4] * a._d[  0] + b._d[  5] * a._d[  4] + b._d[  6] * a._d[  8] + b._d[  7] * a._d[ 12],
		   b._d[  4] * a._d[  1] + b._d[  5] * a._d[  5] + b._d[  6] * a._d[  9] + b._d[  7] * a._d[ 13],
		   b._d[  4] * a._d[  2] + b._d[  5] * a._d[  6] + b._d[  6] * a._d[ 10] + b._d[  7] * a._d[ 14],
		   b._d[  4] * a._d[  3] + b._d[  5] * a._d[  7] + b._d[  6] * a._d[ 11] + b._d[  7] * a._d[ 15],
		
		   b._d[  8] * a._d[  0] + b._d[  9] * a._d[  4] + b._d[ 10] * a._d[  8] + b._d[ 11] * a._d[ 12],
		   b._d[  8] * a._d[  1] + b._d[  9] * a._d[  5] + b._d[ 10] * a._d[  9] + b._d[ 11] * a._d[ 13],
		   b._d[  8] * a._d[  2] + b._d[  9] * a._d[  6] + b._d[ 10] * a._d[ 10] + b._d[ 11] * a._d[ 14],
		   b._d[  8] * a._d[  3] + b._d[  9] * a._d[  7] + b._d[ 10] * a._d[ 11] + b._d[ 11] * a._d[ 15],
		
		   b._d[ 12] * a._d[  0] + b._d[ 13] * a._d[  4] + b._d[ 14] * a._d[  8] + b._d[ 15] * a._d[ 12],
		   b._d[ 12] * a._d[  1] + b._d[ 13] * a._d[  5] + b._d[ 14] * a._d[  9] + b._d[ 15] * a._d[ 13],
		   b._d[ 12] * a._d[  2] + b._d[ 13] * a._d[  6] + b._d[ 14] * a._d[ 10] + b._d[ 15] * a._d[ 14],
		   b._d[ 12] * a._d[  3] + b._d[ 13] * a._d[  7] + b._d[ 14] * a._d[ 11] + b._d[ 15] * a._d[ 15]);
    }

inline GLMatrix operator*( float *const a, const GLMatrix &b) {
  return GLMatrix( b._d[  0] * a[  0] + b._d[  1] * a[  4] + b._d[  2] * a[  8] + b._d[  3] * a[ 12],
		   b._d[  0] * a[  1] + b._d[  1] * a[  5] + b._d[  2] * a[  9] + b._d[  3] * a[ 13],
		   b._d[  0] * a[  2] + b._d[  1] * a[  6] + b._d[  2] * a[ 10] + b._d[  3] * a[ 14],
		   b._d[  0] * a[  3] + b._d[  1] * a[  7] + b._d[  2] * a[ 11] + b._d[  3] * a[ 15],
		
		   b._d[  4] * a[  0] + b._d[  5] * a[  4] + b._d[  6] * a[  8] + b._d[  7] * a[ 12],
		   b._d[  4] * a[  1] + b._d[  5] * a[  5] + b._d[  6] * a[  9] + b._d[  7] * a[ 13],
		   b._d[  4] * a[  2] + b._d[  5] * a[  6] + b._d[  6] * a[ 10] + b._d[  7] * a[ 14],
		   b._d[  4] * a[  3] + b._d[  5] * a[  7] + b._d[  6] * a[ 11] + b._d[  7] * a[ 15],
		
		   b._d[  8] * a[  0] + b._d[  9] * a[  4] + b._d[ 10] * a[  8] + b._d[ 11] * a[ 12],
		   b._d[  8] * a[  1] + b._d[  9] * a[  5] + b._d[ 10] * a[  9] + b._d[ 11] * a[ 13],
		   b._d[  8] * a[  2] + b._d[  9] * a[  6] + b._d[ 10] * a[ 10] + b._d[ 11] * a[ 14],
		   b._d[  8] * a[  3] + b._d[  9] * a[  7] + b._d[ 10] * a[ 11] + b._d[ 11] * a[ 15],
		
		   b._d[ 12] * a[  0] + b._d[ 13] * a[  4] + b._d[ 14] * a[  8] + b._d[ 15] * a[ 12],
		   b._d[ 12] * a[  1] + b._d[ 13] * a[  5] + b._d[ 14] * a[  9] + b._d[ 15] * a[ 13],
		   b._d[ 12] * a[  2] + b._d[ 13] * a[  6] + b._d[ 14] * a[ 10] + b._d[ 15] * a[ 14],
		   b._d[ 12] * a[  3] + b._d[ 13] * a[  7] + b._d[ 14] * a[ 11] + b._d[ 15] * a[ 15]);
    }

inline GLMatrix operator*( const GLMatrix &a, float *const b) {
  return GLMatrix( b[  0] * a._d[  0] + b[  1] * a._d[  4] + b[  2] * a._d[  8] + b[  3] * a._d[ 12],
		   b[  0] * a._d[  1] + b[  1] * a._d[  5] + b[  2] * a._d[  9] + b[  3] * a._d[ 13],
		   b[  0] * a._d[  2] + b[  1] * a._d[  6] + b[  2] * a._d[ 10] + b[  3] * a._d[ 14],
		   b[  0] * a._d[  3] + b[  1] * a._d[  7] + b[  2] * a._d[ 11] + b[  3] * a._d[ 15],
		
		   b[  4] * a._d[  0] + b[  5] * a._d[  4] + b[  6] * a._d[  8] + b[  7] * a._d[ 12],
		   b[  4] * a._d[  1] + b[  5] * a._d[  5] + b[  6] * a._d[  9] + b[  7] * a._d[ 13],
		   b[  4] * a._d[  2] + b[  5] * a._d[  6] + b[  6] * a._d[ 10] + b[  7] * a._d[ 14],
		   b[  4] * a._d[  3] + b[  5] * a._d[  7] + b[  6] * a._d[ 11] + b[  7] * a._d[ 15],
		
		   b[  8] * a._d[  0] + b[  9] * a._d[  4] + b[ 10] * a._d[  8] + b[ 11] * a._d[ 12],
		   b[  8] * a._d[  1] + b[  9] * a._d[  5] + b[ 10] * a._d[  9] + b[ 11] * a._d[ 13],
		   b[  8] * a._d[  2] + b[  9] * a._d[  6] + b[ 10] * a._d[ 10] + b[ 11] * a._d[ 14],
		   b[  8] * a._d[  3] + b[  9] * a._d[  7] + b[ 10] * a._d[ 11] + b[ 11] * a._d[ 15],
		
		   b[ 12] * a._d[  0] + b[ 13] * a._d[  4] + b[ 14] * a._d[  8] + b[ 15] * a._d[ 12],
		   b[ 12] * a._d[  1] + b[ 13] * a._d[  5] + b[ 14] * a._d[  9] + b[ 15] * a._d[ 13],
		   b[ 12] * a._d[  2] + b[ 13] * a._d[  6] + b[ 14] * a._d[ 10] + b[ 15] * a._d[ 14],
		   b[ 12] * a._d[  3] + b[ 13] * a._d[  7] + b[ 14] * a._d[ 11] + b[ 15] * a._d[ 15]);
    }

inline Punto operator*( const GLMatrix &a, const Punto &p) {
  return Punto( p.x() * a._d[  0] + p.y() * a._d[  4] + p.z() * a._d[  8],
		p.x() * a._d[  1] + p.y() * a._d[  5] + p.z() * a._d[  9],
		p.x() * a._d[  2] + p.y() * a._d[  6] + p.z() * a._d[ 10]);
}

inline void MultMatVector4( const GLMatrix &a, float *const p, float *q) {
  q[ 0]	= p[ 0]	* a[  0] + p[ 1] * a[  4] + p[ 2] * a[  8] + p[ 3] * a[ 12];
  q[ 1]	= p[ 0]	* a[  1] + p[ 1] * a[  5] + p[ 2] * a[  9] + p[ 3] * a[ 13];
  q[ 2]	= p[ 0]	* a[  2] + p[ 1] * a[  6] + p[ 2] * a[ 10] + p[ 3] * a[ 14];
  q[ 3]	= p[ 0]	* a[  3] + p[ 1] * a[  7] + p[ 2] * a[ 11] + p[ 3] * a[ 15];
  // Ahora normalizamos el punto:
  if ( fabs( q[ 3]) > CASI_CERO) {
    if ( q[ 3] != 1.0f) {
      float dem = 1.0f / q[ 3];
      q[ 0] *= dem;
      q[ 1] *= dem;
      q[ 2] *= dem;
      q[ 3] *= dem;
    }
  } else {
  }
}

inline void MultMatPunto4( const GLMatrix &a, Punto &p, Punto &q) {
  float r[ 4];
  r[ 0]	= p.x() * a[  0] +
    p.y() * a[  4] +
    p.z() * a[  8] +
    a[ 12];
  r[ 1]	= p.x() * a[  1] + p.y() * a[  5] + p.z() * a[  9] + a[ 13];
  r[ 2]	= p.x() * a[  2] + p.y() * a[  6] + p.z() * a[ 10] + a[ 14];
  r[ 3]	= p.x() * a[  3] + p.y() * a[  7] + p.z() * a[ 11] + a[ 15];
  // assert( r[ 3] == 1.0);
  // if ( r[ 3] != 1.0) {
  //   TclWarningWindow( "Ehh, != 1.0");
  // }
  q.set( r[ 0], r[ 1], r[ 2]);
}

/*
 * Compute inverse of 4x4 transformation matrix.
 * Code contributed by Jacques Leroy jle@star.be
 * Return true for success, false for failure (singular matrix)
 *
 * Could be optimized for special MATRIX_xxx types, especially since
 * this is mainly called for the modelview matrix, which is usually
 * length and angle preserving.
 */
inline bool GLMatrix::get_inverse( float *out) const {
  /* NB. OpenGL Matrices are COLUMN major. */
#define SWAP_ROWS(a, b) { float *_tmp = a; (a)=(b); (b)=_tmp; }
#define MAT(m,r,c) (m)[((c)<<2)+(r)]

  float wtmp[4][8];
  float m0, m1, m2, m3, s;
  float *r0, *r1, *r2, *r3;
  const float *m = _d;

  r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];

  r0[0] = MAT(m,0,0), r0[1] = MAT(m,0,1),
  r0[2] = MAT(m,0,2), r0[3] = MAT(m,0,3),
  r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0f,

  r1[0] = MAT(m,1,0), r1[1] = MAT(m,1,1),
  r1[2] = MAT(m,1,2), r1[3] = MAT(m,1,3),
  r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0f,

  r2[0] = MAT(m,2,0), r2[1] = MAT(m,2,1),
  r2[2] = MAT(m,2,2), r2[3] = MAT(m,2,3),
  r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0f,

  r3[0] = MAT(m,3,0), r3[1] = MAT(m,3,1),
  r3[2] = MAT(m,3,2), r3[3] = MAT(m,3,3),
  r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0f;

  /* choose pivot - or die */
  if (fabs(r3[0])>fabs(r2[0])) SWAP_ROWS(r3, r2);
  if (fabs(r2[0])>fabs(r1[0])) SWAP_ROWS(r2, r1);
  if (fabs(r1[0])>fabs(r0[0])) SWAP_ROWS(r1, r0);
  if (0.0f == r0[0])  return false;

  /* eliminate first variable     */
  m1 = r1[0]/r0[0]; m2 = r2[0]/r0[0]; m3 = r3[0]/r0[0];
  s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s;
  s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s;
  s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s;
  s = r0[4];
  if (s != 0.0f) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; }
  s = r0[5];
  if (s != 0.0f) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; }
  s = r0[6];
  if (s != 0.0f) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; }
  s = r0[7];
  if (s != 0.0f) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; }

  /* choose pivot - or die */
  if (fabs(r3[1])>fabs(r2[1])) SWAP_ROWS(r3, r2);
  if (fabs(r2[1])>fabs(r1[1])) SWAP_ROWS(r2, r1);
  if (0.0f == r1[1])  return false;

  /* eliminate second variable */
  m2 = r2[1]/r1[1]; m3 = r3[1]/r1[1];
  r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2];
  r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3];
  s = r1[4]; if (0.0f != s) { r2[4] -= m2 * s; r3[4] -= m3 * s; }
  s = r1[5]; if (0.0f != s) { r2[5] -= m2 * s; r3[5] -= m3 * s; }
  s = r1[6]; if (0.0f != s) { r2[6] -= m2 * s; r3[6] -= m3 * s; }
  s = r1[7]; if (0.0f != s) { r2[7] -= m2 * s; r3[7] -= m3 * s; }

  /* choose pivot - or die */
  if (fabs(r3[2])>fabs(r2[2])) SWAP_ROWS(r3, r2);
  if (0.0f == r2[2])  return false;

  /* eliminate third variable */
  m3 = r3[2]/r2[2];
  r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4],
  r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6],
  r3[7] -= m3 * r2[7];

  /* last check */
  if (0.0f == r3[3]) return false;

  s = 1.0f / r3[3];              /* now back substitute row 3 */
  r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s;

  m2 = r2[3];                 /* now back substitute row 2 */
  s  = 1.0f / r2[2];
  r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2),
  r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2);
  m1 = r1[3];
  r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1,
  r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1;
  m0 = r0[3];
  r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0,
  r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0;

  m1 = r1[2];                 /* now back substitute row 1 */
  s  = 1.0f / r1[1];
  r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1),
  r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1);
  m0 = r0[2];
  r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0,
  r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0;

  m0 = r0[1];                 /* now back substitute row 0 */
  s  = 1.0f / r0[0];
  r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0),
  r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0);

  MAT(out,0,0) = r0[4]; MAT(out,0,1) = r0[5],
  MAT(out,0,2) = r0[6]; MAT(out,0,3) = r0[7],
  MAT(out,1,0) = r1[4]; MAT(out,1,1) = r1[5],
  MAT(out,1,2) = r1[6]; MAT(out,1,3) = r1[7],
  MAT(out,2,0) = r2[4]; MAT(out,2,1) = r2[5],
  MAT(out,2,2) = r2[6]; MAT(out,2,3) = r2[7],
  MAT(out,3,0) = r3[4]; MAT(out,3,1) = r3[5],
  MAT(out,3,2) = r3[6]; MAT(out,3,3) = r3[7];

  return true;

#undef MAT
#undef SWAP_ROWS
}

// ( 0, 0) ( 0, 1) ( 0, 2) ( 0, 3)
// ( 1, 0) ( 1, 1) ( 1, 2) ( 1, 3)
// ( 2, 0) ( 2, 1) ( 2, 2) ( 2, 3)
// ( 3, 0) ( 3, 1) ( 3, 2) ( 3, 3)

inline float GLMatrix::get_determinant() const {
#define MAT(m,r,c) (m)[((c)<<2)+(r)]
  float deter = \
    MAT( _d, 0, 0) * ( MAT( _d, 1, 3) * MAT( _d, 2, 1) * MAT( _d, 3, 2) - MAT( _d, 1, 3) * MAT( _d, 2, 2) * MAT( _d, 3, 1) +
		       MAT( _d, 1, 2) * MAT( _d, 2, 3) * MAT( _d, 3, 1) - MAT( _d, 1, 2) * MAT( _d, 2, 1) * MAT( _d, 3, 3) +
		       MAT( _d, 1, 1) * MAT( _d, 2, 2) * MAT( _d, 3, 3) - MAT( _d, 1, 1) * MAT( _d, 2, 3) * MAT( _d, 3, 2)) -

    MAT( _d, 1, 0) * ( MAT( _d, 0, 3) * MAT( _d, 2, 1) * MAT( _d, 3, 2) - MAT( _d, 0, 3) * MAT( _d, 2, 2) * MAT( _d, 3, 1) +
		       MAT( _d, 0, 2) * MAT( _d, 2, 3) * MAT( _d, 3, 1) - MAT( _d, 0, 2) * MAT( _d, 2, 1) * MAT( _d, 3, 3) +
		       MAT( _d, 0, 1) * MAT( _d, 2, 2) * MAT( _d, 3, 3) - MAT( _d, 0, 1) * MAT( _d, 2, 3) * MAT( _d, 3, 2)) +

    MAT( _d, 2, 0) * ( MAT( _d, 0, 3) * MAT( _d, 1, 1) * MAT( _d, 3, 2) - MAT( _d, 0, 3) * MAT( _d, 1, 2) * MAT( _d, 3, 1) +
		       MAT( _d, 0, 2) * MAT( _d, 1, 3) * MAT( _d, 3, 1) - MAT( _d, 0, 2) * MAT( _d, 1, 1) * MAT( _d, 3, 3) +
		       MAT( _d, 0, 1) * MAT( _d, 1, 2) * MAT( _d, 3, 3) - MAT( _d, 0, 1) * MAT( _d, 1, 3) * MAT( _d, 3, 2)) -

    MAT( _d, 3, 0) * ( MAT( _d, 0, 3) * MAT( _d, 1, 1) * MAT( _d, 2, 2) - MAT( _d, 0, 3) * MAT( _d, 1, 2) * MAT( _d, 2, 1) +
		       MAT( _d, 0, 2) * MAT( _d, 1, 3) * MAT( _d, 2, 1) - MAT( _d, 0, 2) * MAT( _d, 1, 1) * MAT( _d, 2, 3) +
		       MAT( _d, 0, 1) * MAT( _d, 1, 2) * MAT( _d, 2, 3) - MAT( _d, 0, 1) * MAT( _d, 1, 3) * MAT( _d, 2, 2));
  return deter;
#undef MAT
}

inline bool GLMatrix::get_transpose( GLMatrix &out) const {
#define SWAP_CELLS(out,d,i,j) { float tmp = d[ i]; out[ i] = d[ j]; out[ j] = tmp; }
  out._d[  0] = _d[  0];
  out._d[  5] = _d[  5];
  out._d[ 10] = _d[ 10];
  out._d[ 15] = _d[ 15];
  SWAP_CELLS(out._d,_d,1,4);
  SWAP_CELLS(out._d,_d,2,8);
  SWAP_CELLS(out._d,_d,3,12);
  SWAP_CELLS(out._d,_d,6,9);
  SWAP_CELLS(out._d,_d,7,13);
  SWAP_CELLS(out._d,_d,11,14);

  return true;

#undef SWAP_CELLS
}

inline void GLMatrix::sort() {
  // sorts rows acording to first column, from bigger to smallest
#define MAT(m,r,c) (m)[((c)<<2)+(r)]
  for( int i = 0; i < 4; i++) {
    float key = fabsf( MAT( _d, i, 0));
    for ( int j = i; j < 4; j++) {
      if ( fabsf( MAT( _d, j, 0)) > key) {
        // swap rows
        key = fabsf( MAT( _d, j, 0));
        for ( int k = 0; k < 4; k++) {
          float tmp = MAT( _d, i, k);
          MAT( _d, i, k) = MAT( _d, j, k);
          MAT( _d, j, k) = tmp;
        }
      }
    }
  }
#undef MAT
}

#endif // _PUNTO_H
